/**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       shoot.c/h
  * @brief      射击功能.
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. 完成
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */

#include "shoot.h"
#include "main.h"

#include "cmsis_os.h"
#include "bsp_laser.h"
#include "bsp_fric.h"
#include "arm_math.h"
#include "user_lib.h"
#include "referee.h"
#include "bsp_servo_pwm.h"

#include "CAN_receive.h"
#include "gimbal_behaviour.h"
#include "detect_task.h"
#include "pid.h"

/* 拨弹遥控死区过渡 */
#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }
		
#define shoot_fric1_on(pwm) fric1_on((pwm)) // 摩擦轮1pwm宏定义
#define shoot_fric2_on(pwm) fric2_on((pwm)) // 摩擦轮2pwm宏定义
#define shoot_fric_off() fric_off()         // 关闭两个摩擦轮

#define shoot_laser_on() laser_on()   // 激光开启宏定义
#define shoot_laser_off() laser_off() // 激光关闭宏定义
// 微动开关IO
#define BUTTEN_TRIG_PIN HAL_GPIO_ReadPin(BUTTON_TRIG_GPIO_Port, BUTTON_TRIG_Pin)
// 弹仓盖开关IO
#define shoot_open() servo_pwm_set(500, 0); // 弹仓盖开启
#define shoot_close() servo_pwm_set(1200, 0); // 弹仓盖关闭

float ecd, last_ecd;
int ramp_fric_max ;
int32_t totalAngle; // 累计转过的编码器值

/**
 * @brief          射击状态机设置，遥控器上拨一次开启，再上拨关闭，下拨1次发射1颗，一直处在下，则持续发射，用于3min准备时间清理子弹
 * @param[in]      void
 * @retval         void
 */
static void shoot_set_mode(void);
/**
 * @brief          射击数据更新
 * @param[in]      void
 * @retval         void
 */
static void shoot_feedback_update(void);

/**
 * @brief          堵转倒转处理
 * @param[in]      void
 * @retval         void
 */
static void trigger_motor_turn_back(void);

/**
 * @brief          射击控制，控制拨弹电机角度，完成一次发射
 * @param[in]      void
 * @retval         void
 */
static void shoot_bullet_control(void);

/**
 * @brief          检查射击错误
 * @param[in]      void
 * @retval         void
 */
static void check_shoot_error(void);

shoot_control_t shoot_control; // 射击数据
shoot_error_t shoot_error;     // 射击错误

/**
 * @brief          射击初始化，初始化PID，遥控器指针，电机指针
 * @param[in]      void
 * @retval         返回空
 */
void shoot_init(void)
{
    // PID参数初始化
    static const fp32 Trigger_pid_speed[3] = {TRIGGER_SPEED_PID_KP, TRIGGER_SPEED_PID_KI, TRIGGER_SPEED_PID_KD};
    static const fp32 Trigger_pid_angle[3] = {TRIGGER_ANGLE_PID_KP, TRIGGER_ANGLE_PID_KI, TRIGGER_ANGLE_PID_KD};
    static const fp32 Fric_pid_speed[3] = {FRIC_SPEED_PID_KP, FRIC_SPEED_PID_KI, FRIC_SPEED_PID_KD};
    // 摩擦轮滤波器参数初始化
    const static fp32 fric_filter_num[1] = {2.0};
    first_order_filter_init(&shoot_control.fric1_filter, GIMBAL_CONTROL_TIME, fric_filter_num);
    first_order_filter_init(&shoot_control.fric2_filter, GIMBAL_CONTROL_TIME, fric_filter_num);
    // 摩擦轮斜波初始化
    ramp_init(&shoot_control.fric_ramp, GIMBAL_CONTROL_TIME, SHOOT_FRIC_MAX_SPEED_LOW, SHOOT_FRIC_MIN_SPEED);
    //射击模式初始化
    shoot_control.shoot_mode = SHOOT_STOP;
    // 遥控器指针
    shoot_control.shoot_rc = get_remote_control_point();
    // 电机指针
    shoot_control.shoot_motor_measure = get_trigger_motor_measure_point();
    shoot_control.fric1_motor_measure = get_fric1_motor_measure_point();
    shoot_control.fric2_motor_measure = get_fric2_motor_measure_point();
		shoot_control.cv_ctrl = get_Nano_CV_point();
    // 初始化PID
    PID_init(&shoot_control.trigger_motor_pid_speed, PID_POSITION, Trigger_pid_speed, TRIGGER_READY_PID_MAX_OUT, TRIGGER_READY_PID_MAX_IOUT);
    PID_init(&shoot_control.trigger_motor_pid_angle, PID_POSITION, Trigger_pid_angle, TRIGGER_BULLET_PID_MAX_OUT, TRIGGER_BULLET_PID_MAX_IOUT);
    PID_init(&shoot_control.fric1_motor_pid_speed, PID_POSITION, Fric_pid_speed, FRIC_BULLET_PID_MAX_OUT, FRIC_BULLET_PID_MAX_IOUT);
    PID_init(&shoot_control.fric2_motor_pid_speed, PID_POSITION, Fric_pid_speed, FRIC_BULLET_PID_MAX_OUT, FRIC_BULLET_PID_MAX_IOUT);
    // 更新数据
    shoot_feedback_update();
    // 初始化射击数据
    shoot_control.ecd_count = 0;
    shoot_control.angle = shoot_control.shoot_motor_measure->ecd * 0.000003390842013888;
    shoot_control.trigger_given_current = 0;
    shoot_control.move_flag = 0;
    shoot_control.set_angle = shoot_control.angle;
    shoot_control.speed = 0.0f;
    shoot_control.speed_set = 0.0f;
    shoot_control.key_time = 0;
    ecd = 0, last_ecd = 0;
    shoot_control.stay_shoot_time = 0;
    shoot_control.fric1_speed = 0;
    shoot_control.fric2_speed = 0;
    shoot_control.fric1_speed_set = 0;
    shoot_control.fric2_speed_set = 0;
    shoot_control.fric1_given_current = 0;
    shoot_control.fric2_given_current = 0;
}

/**
 * @brief          射击循环
 * @param[in]      void
 * @retval         返回can控制值
 */
int16_t shoot_control_loop(void)
{

    shoot_set_mode();        // 设置状态机
    shoot_feedback_update(); // 更新数据

    if (shoot_control.shoot_mode == SHOOT_STOP)
    {
        // 设置拨弹轮的速度
        shoot_control.speed_set = 0.0f;
    }
    else if (shoot_control.shoot_mode == SHOOT_READY_FRIC)
    {
        // 设置拨弹轮的速度
        shoot_control.speed_set = 0.0f;
    }
    else if (shoot_control.shoot_mode == SHOOT_READY)
    {
        shoot_control.speed_set = 0.0f;
        ramp_fric_max = 1000;
    }
    else if (shoot_control.shoot_mode == SHOOT_BULLET)
    {
        //        shoot_control.trigger_motor_pid_angle.max_out = TRIGGER_BULLET_PID_MAX_OUT;
        //        shoot_control.trigger_motor_pid_angle.max_iout = TRIGGER_BULLET_PID_MAX_IOUT;
        shoot_bullet_control();
        //防止射击状态卡死在单发模式
        shoot_control.stay_shoot_time++;
        if (shoot_control.stay_shoot_time > 500)
        {
            shoot_control.move_flag = 0;
            shoot_control.shoot_mode = SHOOT_READY_FRIC;
            shoot_control.stay_shoot_time = 0;
        }
    }
    else if (shoot_control.shoot_mode != SHOOT_BULLET)
    {
        shoot_control.stay_shoot_time = 0;
    }
    if (shoot_control.shoot_mode == SHOOT_CONTINUE_BULLET)
    {
        // 设置拨弹轮的拨动速度,并开启堵转反转处理
        shoot_control.trigger_speed_set = CONTINUE_TRIGGER_SPEED;
        trigger_motor_turn_back();
    }
    // 进入反转状态，手动反转防堵转
    if (shoot_control.shoot_mode == SHOOT_TURN_BACK)
    {
        shoot_control.trigger_speed_set = -CONTINUE_TRIGGER_SPEED;
        trigger_motor_turn_back();
        if (shoot_control.shoot_rc->key.v & SHOOT_TURN_BACK_KEY)
        {
            shoot_control.shoot_mode = SHOOT_TURN_BACK;
        }
        else
        {
            shoot_control.shoot_mode = SHOOT_READY_FRIC;
        }
    }
    if (shoot_control.shoot_mode == SHOOT_AUTO)
    {
        if(gimbal_behaviour == GIMBAL_CV_AUTO)
        {
//             if (shoot_control.shoot_cv->Fire_cmd == 1)
//            {
//                 shoot_control.shoot_mode = SHOOT_BULLET;
//             }
//            if (shoot_control.shoot_add_time == AUTO_SHOOT_TIME)
//             {
//                 shoot_control.shoot_mode = SHOOT_CONTINUE_BULLET;
//            }
        }
         else
        {
            shoot_control.shoot_mode = SHOOT_READY_FRIC;
         }
    }
    if (shoot_control.shoot_mode == SHOOT_STOP)
    {
				// shoot_close();
        shoot_laser_off();
        ramp_calc(&shoot_control.fric_ramp, -5);
        shoot_control.fric1_speed_set = shoot_control.fric_ramp.out;
        shoot_control.fric2_speed_set = shoot_control.fric_ramp.out;
        shoot_control.trigger_given_current = 0;
        shoot_control.move_flag = 0;
    }
    else
    {
				// shoot_open();
        shoot_laser_on(); // 激光开启
        // 计算拨弹轮电机PID和计算摩擦轮电机PID，使用斜波函数
        // 摩擦轮需要一个个斜波开启，不能同时直接开启，否则可能电机不转
        static uint16_t car_id;
        robot_id(&car_id);
        if(car_id ==3 || car_id == 103)
        {
            shoot_control.fric_ramp.max_value = SHOOT_FRIC_MAX_SPEED_HIGH;
        }
        else if(car_id == 4 || car_id == 104 || car_id == 5 || car_id ==105)
        {
            shoot_control.fric_ramp.max_value = SHOOT_FRIC_MAX_SPEED_LOW;
        }
        ramp_calc(&shoot_control.fric_ramp, 5);
        shoot_control.fric1_speed_set = -shoot_control.fric_ramp.out;
        shoot_control.fric2_speed_set = shoot_control.fric_ramp.out;

        PID_calc(&shoot_control.trigger_motor_pid_speed, shoot_control.speed, shoot_control.speed_set);

        shoot_control.trigger_given_current = (int16_t)(shoot_control.trigger_motor_pid_speed.out);

        if (shoot_control.shoot_mode < SHOOT_READY_BULLET)
        {
            shoot_control.trigger_given_current = 0;
        }
    }

    PID_calc(&shoot_control.fric1_motor_pid_speed, shoot_control.fric1_speed, shoot_control.fric1_speed_set);
    PID_calc(&shoot_control.fric2_motor_pid_speed, shoot_control.fric2_speed, -shoot_control.fric1_speed);

    shoot_control.fric1_given_current = (int16_t)(shoot_control.fric1_motor_pid_speed.out);
    shoot_control.fric2_given_current = (int16_t)(shoot_control.fric2_motor_pid_speed.out);

    check_shoot_error();
    return shoot_control.trigger_given_current;
}

/**
 * @brief          射击状态机设置，遥控器上拨一次开启，再上拨关闭，下拨1次发射1颗，一直处在下，则持续发射，用于3min准备时间清理子弹
 * @param[in]      void
 * @retval         void
 */
static void shoot_set_mode(void)
{
    static int8_t last_s = RC_SW_UP;
		static int16_t shoot_channel = 0;
	
		/* 获取俯仰控制拨轮通道值 */
		rc_deadband_limit(shoot_control.shoot_rc->rc.ch[SHOOT_CHANNEL], shoot_channel, RC_DEADBAND);
	
    // 上拨判断， 一次开启，再次关闭
    if ((switch_is_up(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]) && !switch_is_up(last_s) && shoot_control.shoot_mode == SHOOT_STOP))
    {
        shoot_control.shoot_mode = SHOOT_READY;
    }
    else if ((switch_is_up(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]) && !switch_is_up(last_s) && shoot_control.shoot_mode != SHOOT_STOP))
    {
        shoot_control.shoot_mode = SHOOT_STOP;
    }

    // 处于中档， 可以使用键盘开启摩擦轮
    if (switch_is_mid(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]) && (shoot_control.shoot_rc->key.v & SHOOT_ON_KEYBOARD) && shoot_control.shoot_mode == SHOOT_STOP)
    {
        shoot_control.shoot_mode = SHOOT_READY_FRIC;
    }
    // 处于中档， 可以使用键盘关闭摩擦轮
    else if (switch_is_mid(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]) && (shoot_control.shoot_rc->key.v & SHOOT_OFF_KEYBOARD) && shoot_control.shoot_mode != SHOOT_STOP)
    {
        shoot_control.shoot_mode = SHOOT_STOP;
    }
		
    if ((switch_is_mid(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]) && switch_is_up(last_s) && shoot_control.shoot_mode == SHOOT_READY))
    {
        shoot_control.shoot_mode = SHOOT_READY_FRIC;
    }
    if (shoot_control.shoot_mode == SHOOT_CONTINUE_BULLET)
    {
        if ((switch_is_mid(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]) || switch_is_down(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]) ) && (shoot_control.shoot_rc->mouse.press_r == 0))
        {
            shoot_control.shoot_mode = SHOOT_READY_FRIC;
        }
    }
    
		if (shoot_control.shoot_mode == SHOOT_READY_FRIC)
    {
				// 俯仰拨轮轮下拨开始拨弹 若要连发一直下拨即可
				if ((shoot_channel >= SHOOT_LEVEL && shoot_control.rc_SHOOT_time != RC_SHOOT_LONG_TIME) || (shoot_control.press_l && shoot_control.last_press_l == 0))
				{
						shoot_control.shoot_mode = SHOOT_BULLET;
				}
				/* 	向上拨俯仰滚轮 高速连发  */
				else if ((shoot_channel <= -SHOOT_LEVEL) || (shoot_control.press_l_time == PRESS_LONG_TIME))
				{
						shoot_control.shoot_mode = SHOOT_CONTINUE_BULLET;
				}
				/* 一直向下拨着滚轮 低速连发(其实是bug 但感觉可以多一种选择 暂且保留 后续可以选择优化) */
				else if ((shoot_channel >= SHOOT_LEVEL && shoot_control.rc_SHOOT_time == RC_SHOOT_LONG_TIME) || (shoot_control.press_l_time == PRESS_LONG_TIME))
				{ 
						shoot_control.shoot_mode = SHOOT_CONTINUE_BULLET;
				}
				
//        // 鼠标长按一直进入射击状态 保持连发
//        if ((shoot_control.press_l_time == PRESS_LONG_TIME) || (shoot_control.rc_s_time == RC_S_LONG_TIME))
//        {
//            shoot_control.shoot_mode = SHOOT_CONTINUE_BULLET;
//        }
//        // 下拨一次或者鼠标按下一次，进入射击状态
//        else if ((switch_is_down(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]) && !switch_is_down(last_s)) || (shoot_control.press_l && shoot_control.last_press_l == 0))
//        {
//            shoot_control.shoot_mode = SHOOT_BULLET;
//        }
        //按G键手动进入反转状态
        else if (shoot_control.shoot_rc->key.v & SHOOT_TURN_BACK_KEY)
        {
            shoot_control.shoot_mode = SHOOT_TURN_BACK;
        }
//        else if (gimbal_behaviour == GIMBAL_CV_AUTO && shoot_control.cv_ctrl->Receice_Flag)
//        {
//            shoot_control.shoot_mode = SHOOT_BULLET;
//			// trigger_motor_turn_back();
//        }
    }
    //在此处保留热量控制，后续接上裁判系统后可自动根据热量进行射频控制
//    get_shoot_heat1_limit_and_heat1(&shoot_control.heat_limit, &shoot_control.heat);
//    if (!toe_is_error(REFEREE_TOE) && (shoot_control.heat + SHOOT_HEAT_REMAIN_VALUE > shoot_control.heat_limit))
//    {
//        if (shoot_control.shoot_mode == SHOOT_BULLET || shoot_control.shoot_mode == SHOOT_CONTINUE_BULLET)
//        {
//            shoot_control.shoot_mode = SHOOT_READY_FRIC;
//        }
//    }

    // 如果云台状态是 无力状态，就关闭射击
    if (gimbal_cmd_to_shoot_stop())
    {
        shoot_control.shoot_mode = SHOOT_STOP;
    }

    last_s = shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL];
}
/**
 * @brief          射击数据更新
 * @param[in]      void
 * @retval         void
 */
static void shoot_feedback_update(void)
{

    static fp32 speed_fliter_1 = 0.0f;
    static fp32 speed_fliter_2 = 0.0f;
    static fp32 speed_fliter_3 = 0.0f;

    // 拨弹轮电机速度滤波一下
    static const fp32 fliter_num[3] = {1.725709860247969f, -0.75594777109163436f, 0.030237910843665373f};

    // 二阶低通滤波
    speed_fliter_1 = speed_fliter_2;
    speed_fliter_2 = speed_fliter_3;
    speed_fliter_3 = speed_fliter_2 * fliter_num[0] + speed_fliter_1 * fliter_num[1] + (shoot_control.shoot_motor_measure->speed_rpm * MOTOR_RPM_TO_SPEED) * fliter_num[2];
    shoot_control.speed = speed_fliter_3;
		
    // 摩擦轮电机速度比例转换并进行一阶低通滤波
		shoot_control.fric1_speed = shoot_control.fric1_motor_measure->speed_rpm;
		shoot_control.fric2_speed = shoot_control.fric2_motor_measure->speed_rpm;

    first_order_filter_cali(&shoot_control.fric1_filter, shoot_control.fric1_speed);
    first_order_filter_cali(&shoot_control.fric2_filter, shoot_control.fric2_speed);
    shoot_control.fric1_speed = shoot_control.fric1_filter.out;
    shoot_control.fric2_speed = shoot_control.fric2_filter.out;

    int32_t dAngle = 0;
    ecd = shoot_control.shoot_motor_measure->ecd;
    if (ecd - last_ecd > HALF_ECD_RANGE)
    {
        dAngle = -last_ecd - (ECD_RANGE - ecd);
    }
    else if (ecd - last_ecd < -HALF_ECD_RANGE)
    {
        dAngle = ecd + (ECD_RANGE - last_ecd);
    }
    else
    {
        dAngle = ecd - last_ecd;
    }
    // 将角度增量加入计数器
    totalAngle += dAngle;
    // 记录角度
    last_ecd = ecd;
    shoot_control.angle = totalAngle;
    shoot_control.angle = shoot_control.angle * (-0.000003390842013888);

    // 微动开关
    shoot_control.key = BUTTEN_TRIG_PIN;
    // 鼠标按键
    shoot_control.last_press_l = shoot_control.press_l;
    shoot_control.last_press_r = shoot_control.press_r;
    shoot_control.press_l = shoot_control.shoot_rc->mouse.press_l;
    shoot_control.press_r = shoot_control.shoot_rc->mouse.press_r;
    // 长按计时
    if (shoot_control.press_l)
    {
        if (shoot_control.press_l_time < PRESS_LONG_TIME)
        {
            shoot_control.press_l_time++;
        }
    }
    else
    {
        shoot_control.press_l_time = 0;
    }

    // 射击开关下档时间计时
//    if (shoot_control.shoot_mode != SHOOT_STOP && switch_is_down(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]))
//    {

//        if (shoot_control.rc_s_time < RC_S_LONG_TIME)
//        {
//            shoot_control.rc_s_time++;
//        }
//    }
//    else
//    {
//        shoot_control.rc_s_time = 0;
//    }
		
		// 射击开关下档时间计时:从左侧开关改成俯仰滚轮
		if (shoot_control.shoot_mode != SHOOT_STOP && SHOOT_CHANNEL_IS_READY(shoot_control.shoot_rc->rc.ch[SHOOT_CHANNEL]))
		{
				if (shoot_control.rc_SHOOT_time <	RC_SHOOT_LONG_TIME)
				{
						shoot_control.rc_SHOOT_time++;
				}
				else
				{
						shoot_control.rc_SHOOT_time = 0;
				}
		}
		
//    if (shoot_control.shoot_cv->Fire_cmd == 1)
//    {
//        if (shoot_control.fire_cmd_time < AUTO_SHOOT_TIME)
//        {
//            shoot_control.fire_cmd_time++;
//        }        
//    }
//    else
//    {
//        shoot_control.fire_cmd_time = 0;
//    }
}

/**
 * @brief 根据射击控制参数触发电机反转。
 * 
 * 如果块时间小于BLOCK_TIME，则速度设置为触发速度设置。
 * 否则，速度设置为触发速度设置的负值。
 * 
 * 如果速度的绝对值小于BLOCK_TRIGGER_SPEED并且块时间小于BLOCK_TIME，
 * 则块时间增加，反转时间重置为0。
 * 
 * 如果块时间等于BLOCK_TIME并且反转时间小于REVERSE_TIME，
 * 则反转时间增加。
 * 
 * 如果以上条件都不满足，则将块时间重置为0。
 * ！注意：此函数慎用，可能会导致多余的反转，建议使用G建手动清理堵转
 * 
 * ！若不使用此函数，则需手动将shoot_control.speed_set = shoot_control.trigger_speed_set;
 * ! 若不使用此函数，则需手动将shoot_control.speed_set = shoot_control.trigger_speed_set;
 * ！若不使用此函数，则需手动将shoot_control.speed_set = shoot_control.trigger_speed_set;
 */
static void trigger_motor_turn_back(void)
{
    if (shoot_control.block_time < BLOCK_TIME)
    {
        shoot_control.speed_set = shoot_control.trigger_speed_set;
    }
    else
    {
        shoot_control.speed_set = -shoot_control.trigger_speed_set;
    }

    if (fabs(shoot_control.speed) < BLOCK_TRIGGER_SPEED && shoot_control.block_time < BLOCK_TIME)
    {
        shoot_control.block_time++;
        shoot_control.reverse_time = 0;
    }
    else if (shoot_control.block_time == BLOCK_TIME && shoot_control.reverse_time < REVERSE_TIME)
    {
        shoot_control.reverse_time++;
    }
    else
    {
        shoot_control.block_time = 0;
    }
}

/**
 * @brief          射击控制，控制拨弹电机角度，完成一次发射
 * @param[in]      void
 * @retval         void
 */
static void shoot_bullet_control(void)
{
    // 每次拨动 0.13圈的角度
    if (shoot_control.move_flag == 0)
    {
        shoot_control.set_angle = shoot_control.angle + 0.13;
        shoot_control.move_flag = 1;
    }
    // 判断是否到达角度
    if (fabs(shoot_control.set_angle - shoot_control.angle) > 0.02)
    {  
        PID_calc(&shoot_control.trigger_motor_pid_angle, shoot_control.angle, shoot_control.set_angle);

        if ((int16_t)(shoot_control.trigger_motor_pid_angle.out) < TRIGGER_SPEED)
        {
            shoot_control.trigger_speed_set = TRIGGER_SPEED;
        }
        else
        {
            shoot_control.trigger_speed_set = -(int16_t)(shoot_control.trigger_motor_pid_angle.out);
        }

        // trigger_motor_turn_back();
	    shoot_control.speed_set = shoot_control.trigger_speed_set;
    }
    else
    {
        if (shoot_control.shoot_add_time < SHOOT_BULLET_TIME)
        {
            shoot_control.shoot_add_time++;
        }
        else
        {
            shoot_control.shoot_add_time = 0;
            shoot_control.move_flag = 0;
            shoot_control.trigger_speed_set = 0.0;
            shoot_control.shoot_mode = SHOOT_READY_FRIC;
        }
    }
}

/**
 * @brief          检查射击错误
 * @param[in]      void
 * @retval         void
 */
static void check_shoot_error(void)
{
    shoot_error.fric_error = 0;
    shoot_error.trigger_error = 0;
    if(shoot_control.shoot_mode != SHOOT_STOP)
    {
        if(shoot_control.fric1_speed > -1.9f || shoot_control.fric2_speed < 1.9f)
        {
            shoot_error.fric_error = 1;
        }        
    }
    if (shoot_control.shoot_mode == SHOOT_BULLET || shoot_control.shoot_mode == SHOOT_CONTINUE_BULLET)
    {
        if(fabs(shoot_control.speed) < BLOCK_TRIGGER_SPEED)
        {
            shoot_error.trigger_error = 1;
        }
    }    
}
